#include "MyProject.h"
#include "foc.h"

/*FOC算法*/

/*
    FOC 执行链：pwm_update_cb() -> FOC_current() / FOC_voltage() -> enqueue_modulation_timings() -> SVM()
*/



/*****************************************************************************/
uint32_t  motor_error;
float  Ialpha_beta[2];

float2D  Idq_setpoint_;
float2D  Vdq_setpoint_;
//电角度
float  phase_;
//角速度
float  phase_vel_;
float2D  *Idq_setpoint_src_;
float2D  *Vdq_setpoint_src_;
float  *phase_src_;
float  *phase_vel_src_;

float  Id_measured,Iq_measured;   //[A]
float  I_measured_report_filter_k = 1.0f;
float  pi_gains_[2];
float  v_current_control_integral_d = 0.0f; // [V]
float  v_current_control_integral_q = 0.0f; // [V]
float  final_v_alpha = 0.0f; // [V]
float  final_v_beta = 0.0f;  // [V]
float  Ibus;
/*****************************************************************************/
//计算SVPWM，并输出PWM波
bool enqueue_modulation_timings(float mod_alpha, float mod_beta)
{
	float tA, tB, tC;
	if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
	{
		set_error(ERROR_MODULATION_MAGNITUDE);
		return false;
	}
    //输出定时器PWM
	TIM1->CCR1 = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
	TIM1->CCR2 = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
	TIM1->CCR3 = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
	return true;
}
/*****************************************************************************/
bool enqueue_voltage_timings(float v_alpha, float v_beta)
{
	float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
    //得到等幅度调制后的alpha-beta坐标系电压
	float mod_alpha = vfactor * v_alpha;
	float mod_beta = vfactor * v_beta;
	if (!enqueue_modulation_timings(mod_alpha, mod_beta))
		return false;
	return true;
}
/****************************************************************************/
// We should probably make FOC Current call FOC Voltage to avoid duplication.
//当前代码移植0.5.1

bool FOC_voltage(float v_d, float v_q, float pwm_phase)  //pwm_phase是park逆变换时的电角度θ
{
    //计算电角度的cos,sin值
	float c = our_arm_cos_f32(pwm_phase);
	float s = our_arm_sin_f32(pwm_phase);
    //park逆变换得到alpha-bete坐标系下的电压
	float v_alpha = c*v_d - s*v_q;
	float v_beta = c*v_q + s*v_d;
    
	return enqueue_voltage_timings(v_alpha, v_beta);
}
/*****************************************************************************/
//在 ADC 采样完成后被调用，执行 Clark 变换
void on_measurement(uint32_t input_timestamp, Iph_ABC_t *current)
{
	// Clark transform
	Ialpha_beta[0] = current->phA;
	Ialpha_beta[1] = one_by_sqrt3 * (current->phB - current->phC);
	//检查标志位，并进行对应的测量
	if(meas_resis==1)Resistance_on_measurement();
	else if(meas_induc==1)Inductance_on_measurement(input_timestamp);
}
/*****************************************************************************/
//foc复位
//在电机停止或初始化时调用，将 PI 控制器的积分项清零
void foc_reset(void)
{
	v_current_control_integral_d = 0.0f;  //积分清零，否则闭环时电机误动作
	v_current_control_integral_q = 0.0f;
	Ialpha_beta[0] = 0;
	Ialpha_beta[1] = 0;
}
/*****************************************************************************/
//0.5.6版本，clark变换与之后的变换分开处理
//Id_des:目标Id  I_phase:当前电角度
bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase)   //I_phase是park变换时的θ，pwm_phase是park逆变换时的θ
{
	// Park transform
    //计算电角度的正余弦
	float c_I = our_arm_cos_f32(I_phase);
	float s_I = our_arm_sin_f32(I_phase);
    //park
	float Id = c_I * Ialpha_beta[0] + s_I * Ialpha_beta[1];
	float Iq = c_I * Ialpha_beta[1] - s_I * Ialpha_beta[0];
    //滤波
	Iq_measured += I_measured_report_filter_k * (Iq - Iq_measured);
	Id_measured += I_measured_report_filter_k * (Id - Id_measured);
	
	// Current error
    //计算误差
	float Ierr_d = Id_des - Id;
	float Ierr_q = Iq_des - Iq;
	
	// TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
	// PI环控制
	float Vd = v_current_control_integral_d + Ierr_d * pi_gains_[0];
	float Vq = v_current_control_integral_q + Ierr_q * pi_gains_[0];
	//等幅度处理
	float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
	float V_to_mod = 1.0f / mod_to_V;
	float mod_d = V_to_mod * Vd;
	float mod_q = V_to_mod * Vq;
	
	// Vector modulation saturation, lock integrator if saturated
	// TODO make maximum modulation configurable
    //电压饱和与抗积分饱和 (Anti-Windup)：
	float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
	if (mod_scalefactor < 1.0f)
	{
		mod_d *= mod_scalefactor;
		mod_q *= mod_scalefactor;
		// TODO make decayfactor configurable
		v_current_control_integral_d *= 0.99f;
		v_current_control_integral_q *= 0.99f;
	}
	else
	{
		v_current_control_integral_d += Ierr_d * (pi_gains_[1] * current_meas_period);
		v_current_control_integral_q += Ierr_q * (pi_gains_[1] * current_meas_period);
	}
	
	// Compute estimated bus current
	Ibus = mod_d * Id + mod_q * Iq;
	//power_ = vbus_voltage * Ibus;
	
	// Inverse park transform
	float c_p = our_arm_cos_f32(pwm_phase);
	float s_p = our_arm_sin_f32(pwm_phase);
	float mod_alpha = c_p * mod_d - s_p * mod_q;
	float mod_beta = c_p * mod_q + s_p * mod_d;
	
	// Report final applied voltage in stationary frame (for sensorles estimator)
    //计算得到最后的alpha-beta坐标系的电压
	final_v_alpha= mod_to_V * mod_alpha;
	final_v_beta = mod_to_V * mod_beta;
	
	// 应用SVPWM
	if (!enqueue_modulation_timings(mod_alpha, mod_beta))
		return false; // error set inside enqueue_modulation_timings
	
	return true;
}
/*****************************************************************************/
//数据同步
void foc_update(void)
{
	if((Idq_setpoint_src_==NULL)||(Vdq_setpoint_src_==NULL))return;  //刚上电时还没有赋值指针，发送指令“A”才赋值
	
	Idq_setpoint_.d = Idq_setpoint_src_->d;
	Idq_setpoint_.q = Idq_setpoint_src_->q;
	Vdq_setpoint_.d = Vdq_setpoint_src_->d;
	Vdq_setpoint_.q = Vdq_setpoint_src_->q;
	phase_ = *phase_src_;
	phase_vel_ = *phase_vel_src_;
}

int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
    int Sextant;

    if (beta >= 0.0f) {
        if (alpha >= 0.0f) {
            //quadrant I
            if (one_by_sqrt3 * beta > alpha)
                Sextant = 2; //sextant v2-v3
            else
                Sextant = 1; //sextant v1-v2

        } else {
            //quadrant II
            if (-one_by_sqrt3 * beta > alpha)
                Sextant = 3; //sextant v3-v4
            else
                Sextant = 2; //sextant v2-v3
        }
    } else {
        if (alpha >= 0.0f) {
            //quadrant IV
            if (-one_by_sqrt3 * beta > alpha)
                Sextant = 5; //sextant v5-v6
            else
                Sextant = 6; //sextant v6-v1
        } else {
            //quadrant III
            if (one_by_sqrt3 * beta > alpha)
                Sextant = 4; //sextant v4-v5
            else
                Sextant = 5; //sextant v5-v6
        }
    }

    switch (Sextant) {
        // sextant v1-v2
        case 1: {
            // Vector on-times
            float t1 = alpha - one_by_sqrt3 * beta;
            float t2 = two_by_sqrt3 * beta;

            // PWM timings
            *tA = (1.0f - t1 - t2) * 0.5f;
            *tB = *tA + t1;
            *tC = *tB + t2;
        } break;

        // sextant v2-v3
        case 2: {
            // Vector on-times
            float t2 = alpha + one_by_sqrt3 * beta;
            float t3 = -alpha + one_by_sqrt3 * beta;

            // PWM timings
            *tB = (1.0f - t2 - t3) * 0.5f;
            *tA = *tB + t3;
            *tC = *tA + t2;
        } break;

        // sextant v3-v4
        case 3: {
            // Vector on-times
            float t3 = two_by_sqrt3 * beta;
            float t4 = -alpha - one_by_sqrt3 * beta;

            // PWM timings
            *tB = (1.0f - t3 - t4) * 0.5f;
            *tC = *tB + t3;
            *tA = *tC + t4;
        } break;

        // sextant v4-v5
        case 4: {
            // Vector on-times
            float t4 = -alpha + one_by_sqrt3 * beta;
            float t5 = -two_by_sqrt3 * beta;

            // PWM timings
            *tC = (1.0f - t4 - t5) * 0.5f;
            *tB = *tC + t5;
            *tA = *tB + t4;
        } break;

        // sextant v5-v6
        case 5: {
            // Vector on-times
            float t5 = -alpha - one_by_sqrt3 * beta;
            float t6 = alpha - one_by_sqrt3 * beta;

            // PWM timings
            *tC = (1.0f - t5 - t6) * 0.5f;
            *tA = *tC + t5;
            *tB = *tA + t6;
        } break;

        // sextant v6-v1
        case 6: {
            // Vector on-times
            float t6 = -two_by_sqrt3 * beta;
            float t1 = alpha + one_by_sqrt3 * beta;

            // PWM timings
            *tA = (1.0f - t6 - t1) * 0.5f;
            *tC = *tA + t1;
            *tB = *tC + t6;
        } break;
    }

    // if any of the results becomes NaN, result_valid will evaluate to false
    int result_valid =
            *tA >= 0.0f && *tA <= 1.0f
         && *tB >= 0.0f && *tB <= 1.0f
         && *tC >= 0.0f && *tC <= 1.0f;
    return result_valid ? 0 : -1;
}

/*****************************************************************************/
//0.5.6使用了包含函数指针的类，切换不同工作状态，非常不直观。所以移植时更改为标志位判断

//FOC 实时环路的入口点。由ADC中断在每个PWM 周期（8kHz）精确地调用一次。
void pwm_update_cb(void)
{
    //检查标志位（meas_resis, meas_induc）以确定是否处于电机参数测量模式
	if(meas_resis==1)Resistance_get_alpha_beta_output();      //测量电阻时
	else if(meas_induc==1)Inductance_get_alpha_beta_output(); //测量电感时
	else   //如果不是，则进入正常工作
	{
		float pwm_phase = phase_ + 1.5f * current_meas_period * phase_vel_;  //进行超前补偿。
        //通过 phase_vel_（速度）来预测 PWM 脉冲在下一个周期的中间点时电机会转到的角度
		//检查电机类型---云台电机or其他关节电机
		if(motor_config.motor_type == MOTOR_TYPE_GIMBAL)
			FOC_voltage(Vdq_setpoint_.d, Vdq_setpoint_.q, pwm_phase);   //gimbal电机用电压模式
		else
			FOC_current(Idq_setpoint_.d, Idq_setpoint_.q, phase_, pwm_phase);  //highcurrent电机用电流模式
	}
}
/*****************************************************************************/



